using UnityEngine;
using System.Collections;

/**
 * This interface is kind of a kludge
 * Ideally it would have the state baked in.
 * Anyways probably just implement it as follows:
 * 
 * private OrbitCircle ocref;
 * public OrbitCircle oc {
 *	 set { this.ocref = value; }
 *	 get { return this.ocref; }
 * }
 * 
 * The object that is orbiting should implement this interface
 * e.g. if the earth is orbiting the sun, the earth class implements this
 * It makes the most sense if the object is a MonoBehavior, but it does not rely on it
 */
interface IOrbitCircle {
	OrbitCircle oc {
		set;
		get;
	}
}

/**
 * Ideally, this should be a mixin (basically need interface+state)
 * Anyways usage is as follows:
 * 1. The class that orbits (e.g. the earth) implements IOrbitCircle
 * 2. The object at the center of the orbit (e.g. the sun) instantiates new OrbitCircle
 * 3. The object at the center of the orbit sets oc in the interface to the OrbitCircle
 * 4. The child calls oc.GetNextPos with the current position and time delta to get the next position.
 * 
 * e.g.
 * oc = new OrbitCircle();
 * earth = new Earth();
 * earth.oc = oc;
 * 
 * In Earth.Update:
 * if (oc != null) {
 * 	this.transform.position = this.oc.GetNextPos();
 * }
 */
public class OrbitCircle {
	protected const float MIN_DISTANCE = 0.1f;
	protected const float EPSILON = 0.0001f;
	
	protected GameObject CenterObj = null;
	protected Vector3 OriginalUnitNormal = new Vector3(0,0,1);
	
	/**
	 * None of these initial values matter
	 * They should all be set by the constructor
	 */
	protected Vector3 center_position = new Vector3(0,0,0);
	protected Vector3 unit_normal = new Vector3(0,0,1);
	protected Vector3 first_point = new Vector3(1,0,0);
	protected float radius = 1;
	protected float circumference = 0;
	
	public float speed = 1;
	
	public Vector3 UnitNormal {
		get { return this.unit_normal; }
		set {
			this.unit_normal = value;
			this.SetFirstPoint();
		}
	}
		
	public Vector3 CenterPosition {
		get { return this.center_position; }
		set {
			this.center_position = value;
			this.SetFirstPoint();
		}
	}
	
	public float Radius {
		get { return this.radius; }
		set {
			this.radius = value;
			this.SetFirstPoint();
			this.SetCircumference();
		}
	}
	
	/**
	 * @param center_pos: the center of the circle
	 * @param normal: the normal vector that defines the plane
	 * @param radius: the radius of the circle
	 * @param orbit_speed: in meters per second
	 */
	public OrbitCircle(GameObject center, Vector3 normal, float radius, float orbit_speed) {
		this.CenterObj = center;
		this.CenterPosition = center.transform.position;
		this.OriginalUnitNormal = normal.normalized;
		this.UnitNormal = this.OriginalUnitNormal;
		this.Radius = radius;
		this.speed = orbit_speed;
	}
	
	protected virtual void UpdateFromCenter() {
		this.CenterPosition = this.CenterObj.transform.position;
		this.UnitNormal = this.CenterObj.transform.rotation * this.OriginalUnitNormal;
	}
	
	
	/**
	 * @param curr_pos: generally going to be this.transform.position
	 * @param delta_time: generally going to be Time.deltaTime from an Update() call
	 */
	public virtual Vector3 GetNextPos(Vector3 curr_pos, float delta_time) {
		Vector3 dest_pos;
		float angle;
		float distance_to_travel;
		Quaternion rotation;
		
		/**
		 * Update some fields based on the center object's movement
		 */
		this.UpdateFromCenter();
		
		distance_to_travel = this.speed * delta_time;
		if (distance_to_travel < OrbitCircle.MIN_DISTANCE) {
			distance_to_travel = OrbitCircle.MIN_DISTANCE;
		}
		
		/* If we travel 25% of the circumference, then we do e.g. .25 * 360 to get 90 degrees */
		angle = 360.0f * distance_to_travel / this.circumference;
		rotation = Quaternion.AngleAxis (angle, this.unit_normal);
		
		if (this.IsPointInPlane(curr_pos)) {
			/* Already in the plane of orbit */
			Vector3 plane_vector;
			
			dest_pos = this.ApplyQuaternion (curr_pos, rotation);
			
			/* Clamp to radius due to rounding and other issues */
			plane_vector = dest_pos - this.CenterPosition;
			dest_pos = this.CenterPosition + ((plane_vector * this.radius) / plane_vector.magnitude);
		} else {
			/**
			 * Move towards the desired point
			 * Probably needs some work
			 */
			Vector3 expected_pos;
			Vector3 unit_move;
			
			/**
			 * expected_pos is closest point on plane to curr_pos
			 * Use that to calculate the desired destination pos
			 * Then move in a straight line towards the destination
			 */
			expected_pos = this.ApplyQuaternion(this.GetClosestPointInCircle(curr_pos), rotation);
			unit_move = (expected_pos - curr_pos).normalized * distance_to_travel;
			dest_pos = curr_pos + unit_move;	
		}
		
		return dest_pos;
	}
	
	private void SetCircumference() {
		this.circumference = this.radius * 2.0f * Mathf.PI;
	}
	
	private void SetFirstPoint() {
		Vector3 arbitrary;
		Vector3 dir;
		
		/**
		 * 1. Choose an arbitrary axis
		 * 
		 * To avoid some issues with parallel vectors, choose the X,Y,or Z axis
		 * depending on which value of the normal vector is the smallest
		 * 
		 * 2. Take the cross product of the normal vector with the axis.
		 * 3. The result is a vector in the plane of the circle
		 * 4. Use the vector to find a point r units from the center (r is the radius)
		 */
		if (this.unit_normal.x < this.unit_normal.y) {
			if (this.unit_normal.x < this.unit_normal.z) {
				arbitrary = new Vector3(1, 0, 0);
			} else {
				arbitrary = new Vector3(0, 0, 1);
			}
		} else if (this.unit_normal.y < this.unit_normal.z) {
			arbitrary = new Vector3(0, 1, 0);
		} else {
			arbitrary = new Vector3(0, 0, 1);
		}
		
		dir = Vector3.Cross (this.unit_normal, arbitrary).normalized;
		
		this.first_point = (dir*this.radius) + this.CenterPosition;
	}
	
	protected bool IsPointInPlane(Vector3 point) {
		Vector3 plane_vector = point - this.CenterPosition;

		/**
		 * normal vector dot plane vector == 0 iff the vectors are orthogonal
		 * Note: Mathf.EPSILON is too small, we need a slightly bigger value to avoid jitter problems
		 */
		return (Mathf.Abs(Vector3.Dot (this.unit_normal, plane_vector)) < OrbitCircle.EPSILON);
	}
	
	protected Vector3 GetClosestPointInCircle(Vector3 point) {
		float t;
		Vector3 plane_point;
		
		/**
		 * 1. Get the closest point to the plane
		 * 2. Follow the line from center to a point on the circle
		 * 3. If the closest point is the center, use our magic initial vector
		 * 
		 */
		
		/**
		 * Using parametric t:
		 * ret must be on the line from point going in direction unit_normal
		 * ret = t*normal + point
		 * 
		 * ret must be on the plane:
		 * normal dot (ret - center) = 0
		 * normal dot (t*normal + point - center) = 0
		 * t*normal^2 + normal dot (point - center) = 0
		 * t = -[normal dot (point - center) ] / normal^2
		 * Of course, we know normal is a unit normal, so normal^2 = 1
		 */
		t = -1.0f * Vector3.Dot(this.unit_normal, point - this.CenterPosition);

		plane_point = point + (this.unit_normal * t);
		if (plane_point == this.CenterPosition) {
			return this.first_point;
		} else {
			return ((plane_point - this.CenterPosition).normalized * this.radius) + this.CenterPosition;
		}
	}
	
	protected Vector3 ApplyQuaternion(Vector3 point, Quaternion rotation) {
		/* Translate to origin-based coords since rotation is origin-based */
		point = point - this.CenterPosition;
		point = rotation*point;
		/* Translate back to the plane of the circle */
		return point + this.CenterPosition;
	}
}
